#include "data_deal.h"
#include "type_conversion.h"
#include "attitude_control.h"
#include "position_control.h"

/**
* @Description 汇报四旋翼姿态到上位机
 */
void Send_IMU_to_PC(void)
{
 	float temp;
	u8 floatToHex[4];		
	u8 dataToPC[64];	
	u8 i=0;
	
	//上报帧类型
	dataToPC[0]=0X55;
	dataToPC[1]=0XAA;
	dataToPC[2]=0X01;
	
	//姿态是浮点数类型的，需要转换成字节类型的才能通过串口发送
	temp = RT_Info.Pitch - Errangle_Info.fixedErroPitch;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,3,floatToHex,4);
	
	temp = RT_Info.Roll - Errangle_Info.fixedErroRoll;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,7,floatToHex,4);
	
	temp = RT_Info.FlowX_V*100;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,3,floatToHex,4);
	
	temp = RT_Info.FlowY_V*100;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,7,floatToHex,4);
	
	temp = RT_Info.Yaw;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,11,floatToHex,4);
	
	temp = RT_Info.US100_Alt_V;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,15,floatToHex,4);


	temp = RT_Info.batteryVoltage;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,19,floatToHex,4);
	
	temp = RT_Info.FlowY_V * 100;
	FloatToByte(temp,floatToHex);
	arrycat(dataToPC,23,floatToHex,4);
	
	for(i=27;i<47;i++)
	{
		dataToPC[i]=0;
	}
	//计算数据和、用于校验
	for(i=0;i<47;i++)
	{
		dataToPC[47]+=dataToPC[i];
	}
	
	Uart3_to_BT_tx(dataToPC,48);
}

/**
* @Description 汇报四旋翼PID参数到上位机
 */
void Send_Para_to_PC(void)
{
	u8 paraToPC[255];
	u8 floatToHex[4];	
	u8 intToHex[4];
	int i=0;
	//上报帧类型
	paraToPC[0]=0X55;
	paraToPC[1]=0XAA;
	paraToPC[2]=0X02;	
	
	//上报pitch的pid参数
	FloatToByte(Para_Info.Pitch.Kp,floatToHex);
	arrycat(paraToPC,3,floatToHex,4);
	FloatToByte(Para_Info.Pitch.Ki,floatToHex);
	arrycat(paraToPC,7,floatToHex,4);
	FloatToByte(Para_Info.Pitch.Kd,floatToHex);
	arrycat(paraToPC,11,floatToHex,4);

	//上报roll的pid参数
	FloatToByte(Para_Info.Roll.Kp,floatToHex);
	arrycat(paraToPC,15,floatToHex,4);
	FloatToByte(Para_Info.Roll.Ki,floatToHex);
	arrycat(paraToPC,19,floatToHex,4);
	FloatToByte(Para_Info.Roll.Kd,floatToHex);
	arrycat(paraToPC,23,floatToHex,4);

	//上报yaw的pid参数
	FloatToByte(Para_Info.Yaw.Kp,floatToHex);
	arrycat(paraToPC,27,floatToHex,4);
	FloatToByte(Para_Info.Yaw.Ki,floatToHex);
	arrycat(paraToPC,31,floatToHex,4);
	FloatToByte(Para_Info.Yaw.Kd,floatToHex);
	arrycat(paraToPC,35,floatToHex,4);
	
	
	//上报z轴的pid参数
	FloatToByte(Para_Info.Height.Kp,floatToHex);
	arrycat(paraToPC,39,floatToHex,4);
	FloatToByte(Para_Info.Height.Ki,floatToHex);
	arrycat(paraToPC,43,floatToHex,4);
	FloatToByte(Para_Info.Height.Kd,floatToHex);
	arrycat(paraToPC,47,floatToHex,4);
			
	
	//上报pitch角速度的pid参数
	FloatToByte(Para_Info.ratePitch.Kp,floatToHex);
	arrycat(paraToPC,51,floatToHex,4);
	FloatToByte(Para_Info.ratePitch.Ki,floatToHex);
	arrycat(paraToPC,55,floatToHex,4);
	FloatToByte(Para_Info.ratePitch.Kd,floatToHex);
	arrycat(paraToPC,59,floatToHex,4);

	//上报roll角速度的pid参数
	FloatToByte(Para_Info.rateRoll.Kp,floatToHex);
	arrycat(paraToPC,63,floatToHex,4);
	FloatToByte(Para_Info.rateRoll.Ki,floatToHex);
	arrycat(paraToPC,67,floatToHex,4);
	FloatToByte(Para_Info.rateRoll.Kd,floatToHex);
	arrycat(paraToPC,71,floatToHex,4);

	//上报yaw角速度的pid参数
	FloatToByte(Para_Info.rateYaw.Kp,floatToHex);
	arrycat(paraToPC,75,floatToHex,4);
	FloatToByte(Para_Info.rateYaw.Ki,floatToHex);
	arrycat(paraToPC,79,floatToHex,4);
	FloatToByte(Para_Info.rateYaw.Kd,floatToHex);
	arrycat(paraToPC,83,floatToHex,4);

	
	//上报z轴速度的pid参数
	FloatToByte(Para_Info.HeightRate.Kp,floatToHex);
	arrycat(paraToPC,87,floatToHex,4);
	FloatToByte(Para_Info.HeightRate.Ki,floatToHex);
	arrycat(paraToPC,91,floatToHex,4);
	FloatToByte(Para_Info.HeightRate.Kd,floatToHex);
	arrycat(paraToPC,95,floatToHex,4);	
	
	for(i=99;i<157;i++)
	{
		paraToPC[i]=0;
	}	
	
	IntToByte(0.0,intToHex);
	arrycat(paraToPC,157,intToHex,2);
	
	//数据求和校验
	for(i=0;i<159;i++)
	{
		paraToPC[159]+=paraToPC[i];
	}	
	
	Uart3_to_BT_tx(paraToPC,160);
}

//设置pid参数
static void set_pid(PID *pid,unsigned char *buf)
{
	u8 HexToFloat[4];
	float pidParaTemp[3];
	int i,j;
	
	for(i=0;i<3;i++)
	{
		for(j=0;j<4;j++)
		{
			HexToFloat[j]=buf[3+j+i*4];
		}
		pidParaTemp[i]=Hex_To_Decimal(HexToFloat,4);
	}  
	pid->Kp=pidParaTemp[0];
	pid->Ki=pidParaTemp[1];
	pid->Kd=pidParaTemp[2];
	
	FlightControl.ReportSW=Report_SET;
}	


/* 基础油门量 */
volatile float throttleBasic;
/**
* @Description 解析上位机的数据到全局变量
 */
void data_deal(_Data_Rx rx)
{
	u8 HexToFloat[4];	//串口的数据是字节流，一部分需要转换为浮点数
	
	u8 i;
	
	if( rx.len==20 && rx.buf[0]==0x55 && rx.buf[1]==0xAA )
	{
		//无人机开关命令、飞行器不处于低电压
		if(rx.buf[2]==0xff && RT_Info.lowPowerFlag==0) 
		{
			if(rx.buf[3]==0)
				FlightControl.OnOff = Drone_Off;
			else if(rx.buf[3]==1)	//启动无人机
			{	
				//每次开启，都要清零PID以及控制数据	
				Position_PID_Init();			
				Attitude_PID_Init();	
				//开启飞行器					
				FlightControl.OnOff = Drone_On;
			}
			else if(rx.buf[3]==2)	//降落命令		
			{
				//开启降落程序
				FlightControl.landFlag=1;
			}						
		}	
		/*设置目标角度----用于角度控制的飞行or调试姿态外环pid参数*/
		else if(rx.buf[2]==0x01 && FlightControl.droneMode!=Drone_Mode_4Axis ) 
		{
			/*  pitch目标值 */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[3+i];
			}
			Target_Info.Pitch = Hex_To_Decimal(HexToFloat,4); 
			/*  roll目标值 */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[7+i];
			}
			Target_Info.Roll = Hex_To_Decimal(HexToFloat,4);								
			/*  yaw角速度目标值 */					
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[11+i];
			}
			Target_Info.RateYaw = Hex_To_Decimal(HexToFloat,4); 

			/*  z轴目标值 ****************************/
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[15+i];
			}
		}	
		//选择实验飞行模式同时配置基础量
		else if(rx.buf[2]==0x02) 
		{
			switch(rx.buf[3])
			{
				case 0:
						FlightControl.droneMode=Drone_Mode_None;
					break;
				case 1:
						FlightControl.droneMode=Drone_Mode_Pitch;
						throttleBasic = 250;
					break;
				case 2:
						FlightControl.droneMode=Drone_Mode_Roll;
						throttleBasic = 250;					
					break;
				case 3:
						FlightControl.droneMode=Drone_Mode_4Axis; 
						throttleBasic = 520;
					break;
				case 4:
						FlightControl.droneMode=Drone_Mode_RatePitch; 
						throttleBasic = 250;
					break;
				case 5:
						FlightControl.droneMode=Drone_Mode_RateRoll;
						throttleBasic = 250;
				default:
					break;
			}
		}
		//要求飞行器汇报PID参数
		else if(rx.buf[2]==0x03) 
		{
			FlightControl.ReportSW=Report_SET;					
		}
		/*设置Pitch的PID*/	
		else if(rx.buf[2] == 0x05)
		{
			set_pid(&Para_Info.Pitch,rx.buf);
		}				
		/*设置Roll的PID*/					
		else if(rx.buf[2] == 0x06)
		{
			set_pid(&Para_Info.Roll,rx.buf);
		}
		/*设置Yaw的PID*/					
		else if(rx.buf[2] == 0x07)
		{
			set_pid(&Para_Info.Yaw,rx.buf);
		}
		/*设置Height的PID*/					
		else if(rx.buf[2] == 0x08)
		{
			set_pid(&Para_Info.Height,rx.buf);
		}	
		/*水平面角度校准：飞行器起飞前放在水平面、这时有角度数据，这个数据就是偏差，需要在起飞后减去这个偏差*/
		else if(rx.buf[2] == 0x0a)
		{
			//记录当前角度数据为偏差
			Errangle_Info.fixedErroPitch = RT_Info.Pitch;
			Errangle_Info.fixedErroRoll = RT_Info.Roll;
		
			FlightControl.ReportSW=Report_SET;
		}		
	 	/*设置ratePitch的PID*/					
		else if(rx.buf[2] == 11)
		{
			set_pid(&Para_Info.ratePitch,rx.buf);
		}
		/*设置rateRoll的PID*/					
		else if(rx.buf[2] == 12)
		{
			set_pid(&Para_Info.rateRoll,rx.buf);
		}
		/*设置rateYaw的PID*/					
		else if(rx.buf[2] == 13)
		{
			set_pid(&Para_Info.rateYaw,rx.buf);
		}
		/*设置HeighRate的PID*/					
		else if(rx.buf[2] == 14)
		{
			set_pid(&Para_Info.HeightRate,rx.buf);
		}
		/*设置目标角速度 在内环实验中使用*/
		else if(rx.buf[2]==15) 
		{
			/* Target_RatePitch */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[3+i];
			}
			Target_Info.RatePitch = Hex_To_Decimal(HexToFloat,4); 
			/* Target_RateRoll */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[7+i];
			}
			Target_Info.RateRoll = Hex_To_Decimal(HexToFloat,4);										
		}	
		/*手柄遥控信息*/
		else if(rx.buf[2]==17)
		{
			/* Roll 轴的速度目标值 */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[3+i];
			}
			Control_Info.rV = - Hex_To_Decimal(HexToFloat,4); 
			/* Pitch 轴的速度目标值*/
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[7+i];
			}
			Control_Info.pV =  Hex_To_Decimal(HexToFloat,4); 					
			/* Yaw 的角速度目标值 */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[11+i];
			}
			Control_Info.yV = Hex_To_Decimal(HexToFloat,4); 	
			/*Heigh高度增量 */
			for(i=0;i<4;i++)
			{
				HexToFloat[i]=rx.buf[15+i];
			}
			Control_Info.hV = Hex_To_Decimal(HexToFloat,4); 
			
			//每一次高度增加or减少幅度不超过0.3米
			if(Control_Info.hV<0.3f && Control_Info.hV>-0.3f)
			{
				
				float tmpH=Target_Info.Height+Control_Info.hV;		
				//最高目标高度1米3				
				if(tmpH<1.3f)
					Target_Info.Height=tmpH;
				else
					Target_Info.Height=1.3;
			}
		}		
	}
}
